/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.controller.breakaway.teleop;

import com.grt192.core.StepController;
import com.grt192.event.component.EncoderEvent;
import com.grt192.event.component.EncoderListener;
import com.grt192.mechanism.breakaway.GRTBreakawayRobotBase;
import com.grt192.mechanism.GRTDriverStation;
import com.grt192.sensor.GRTEncoder;
import com.grt192.sensor.GRTJoystick;
import com.grt192.utils.Util;

/**
 *
 * @author Bonan, James, Ryo
 */
public class BreakawayTeleopDriveController extends StepController
        implements EncoderListener {

    public static final double SPIN_THRESHOLD = .1;
    private int driveMode = GRTBreakawayRobotBase.TANK_DRIVE;
    private boolean printingData = false;

    public BreakawayTeleopDriveController(GRTBreakawayRobotBase rb, GRTDriverStation ds) {
        super();
        addMechanism("RobotBase", rb);
        addMechanism("DriverStation", ds);
        ((GRTEncoder) rb.getSensor("LeftEncoder")).addEncoderListener(this);
        ((GRTEncoder) rb.getSensor("RightEncoder")).addEncoderListener(this);

        
    }

    public void act() {
        GRTDriverStation ds = (GRTDriverStation) getMechanism("DriverStation");
        GRTBreakawayRobotBase rb =
                (GRTBreakawayRobotBase) getMechanism("RobotBase");

        if (ds.getRightButton(9) && driveMode == GRTBreakawayRobotBase.TANK_DRIVE) {
            driveMode = GRTBreakawayRobotBase.CAR_DRIVE;
        } else if (ds.getRightButton(9) && driveMode == GRTBreakawayRobotBase.CAR_DRIVE){
            driveMode = GRTBreakawayRobotBase.TANK_DRIVE;
        }

        switch (driveMode) {
            case GRTBreakawayRobotBase.TANK_DRIVE:
                rb.tankDrive(ds.getYLeftJoystick(), -ds.getYRightJoystick());
                break;
            case GRTBreakawayRobotBase.CAR_DRIVE:
                rb.carDrive(ds.getXRightJoystick(),ds.getYRightJoystick(),
                        Math.abs(ds.getYRightJoystick()) < SPIN_THRESHOLD);
                break;
            case GRTBreakawayRobotBase.ONE_STICK_DRIVE:
                rb.oneStickDrive(
                        Util.distance(Util.roundValue(
                        ds.getYLeftJoystick()),
                        Util.roundValue(ds.getYRightJoystick())),
                        ((GRTJoystick) ds.getSensor("leftJoystick")).getState("JoystickAngle"));
                        driveMode = GRTBreakawayRobotBase.TANK_DRIVE;
                break;

        }

        if (ds.getLeftButton(6)) {
            printingData = !printingData;
            System.out.println(printingData ? "printing" : "not pringing");
        }
        if (printingData) {
            printData();
        }


    }
    public void printData() {
        GRTBreakawayRobotBase rb =
                (GRTBreakawayRobotBase) getMechanism("RobotBase");
        System.out.println("gryo: " + rb.getGyroAngle());
        System.out.println("x feet: " + rb.getX() / 12.0);
        System.out.println("y feet: " + rb.getY() / 12.0);
    }
    public void countDidChange(EncoderEvent e) {
//        System.out.println(e.getSource().getId() + " " +e.getDistance());

    }

    public void rotationDidStart(EncoderEvent e) {
    }

    public void rotationDidStop(EncoderEvent e) {
    }

    public void changedDirection(EncoderEvent e) {
    }



}

